/**
 ****************************************************************************************************
 * @file        foc_math.c
 * @author      哔哩哔哩-Rebron大侠
 * @version     V0.0
 * @date        2025-01-11
 * @brief       FOC的数学运算
 * @license     MIT License
 *              Copyright (c) 2025 Reborn大侠
 *              允许任何人使用、复制、修改和分发该代码，但需保留此版权声明。
 ****************************************************************************************************
 */

#include "foc_math.h"

int8_t FOC_SIGN_Judgment(float val)
{
    if(val>=0)return 1;
    else return 0;
}

/*克拉克变换*/
alphabeta_t FOC_Clarke(abc_t input)
{
    alphabeta_t output = {0};

    /* qIalpha = qIas*/
    /*qIbeta = -(2*qIbs+qIas)/sqrt(3)*/

    output.alpha = input.a;
    output.beta = ((input.a + 2.0f*input.b)/SQRT_3);
    
    return output;
}

/*帕克变换*/
qd_t Foc_Park(alphabeta_t input, float ElAngle)
{
    qd_t output = {0};

    float _cos = arm_cos_f32(ElAngle);
    float _sin = arm_sin_f32(ElAngle);

    output.q = _cos*input.beta -  _sin*input.alpha;
    output.d = _cos*input.alpha + _sin*input.beta;

    return output;
}

/*帕克逆变换*/
alphabeta_t FOC_Rev_Park(qd_t input, float ElAngle)
{
    alphabeta_t output = {0};

    float _cos = arm_cos_f32(ElAngle);
    float _sin = arm_sin_f32(ElAngle);

    /*派克逆变换
    Iα = Id*cosθ - Iq*sinθ
    Iβ = Iq*cosθ + Id*sinθ 我们取Id为0的情况*/
    output.alpha = _cos* input.d - _sin* input.q;
    output.beta  = _cos* input.q + _sin* input.d;

    return output;
}

/*克拉克逆变换*/
abc_t FOC_Rev_Clarke(alphabeta_t input)
{
    abc_t output = {0};
/*
    Ia = Iα;
    Ib = (√3*Iβ-Iα)/2
    Ic = (-Iα-√3*Iβ)/2 
*/
    output.a = input.alpha;
    output.b = (SQRT_3*(input.beta) -input.alpha)/2.0f;
    output.c = (-input.alpha - SQRT_3*(input.beta))/2.0f;

    return output;
}

/*限制角度在0-2PI之间*/
/*限制角度在0-2PI之间*/
float Limit_Angle(float ElAngle)
{
    if(ElAngle>_2PI){
        ElAngle -= _2PI;
    }else if(ElAngle<0){
        ElAngle += _2PI;
    }
    return ElAngle;
}

/*机械角度转换为电角度
MecAngle:机械角度
pole_pairs：极对数
返回:电角度
*/
float _MecAngle_to_ElAngle(float MecAngle, uint32_t pole_pairs)
{
    return (float)(MecAngle * pole_pairs);
}
